#include <cstdio>

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "epuckDriver/OdometryPublisherMsg.h"
#include "epuckDriver/IRDataPublisherMsg.h"
#include "epuckDriver/CameraDataMsg.h"

#include <unistd.h>

#include <string>

ros::Publisher pub[6];

void VelocityCallBack(const geometry_msgs::Twist::ConstPtr& twist){
    
    for(int i=0;i<6;i++)    
        pub[i].publish(twist);     
}

int main(int argc,char **argv){

    ros::init(argc,argv,"testVelocity");
    ros::NodeHandle n;
    
    ros::Subscriber sub = n.subscribe<geometry_msgs::Twist>("cmd_vel",1,VelocityCallBack);
    
    pub[0] = n.advertise<geometry_msgs::Twist>("epuckVelocity1",1);
    pub[1] = n.advertise<geometry_msgs::Twist>("epuckVelocity2",1);
    pub[2] = n.advertise<geometry_msgs::Twist>("epuckVelocity3",1);
    pub[3] = n.advertise<geometry_msgs::Twist>("epuckVelocity4",1);
    pub[4] = n.advertise<geometry_msgs::Twist>("epuckVelocity5",1);
    pub[5] = n.advertise<geometry_msgs::Twist>("epuckVelocity6",1);    
    
    while(true)        
        ros::spinOnce();
    
    return 0;
}
